/*
 * WorldLines.cpp
 *
 *  Created on: Jan 24, 2010
 *      Author: snirgaz
 */
#include "WorldLines.h"
bool isInRange(double *vec, int size, double lim) {
	for (int p = 0; p < size; p++) {
		if (fabs(vec[p]) > lim)
			return false;
	}
	return true;
}

//void WorldLinesData::calcNearestNeighbors(boost::multi_array<int, 3> &nnIndices,
//		int numOfnn) {
//	double dists[NumP_][NumP_];
//	int indexTemp[NumP_];
//	double diff[Dim_];
//	DoublePointerVec dimP;
//	BeadPos pos1, pos2, delta;
//	int p;
//	for (int m = 0; m < M_; m++) {
//		for (int n1 = 0; n1 < NumP_; n1++) {
//			pos1 = this->getDataPos(n1, m);
//			for (int n2 = n1 + 1; n2 < NumP_; n2++) {
//				pos2 = this->getDataPos(n2, m);
//				pos1.subBeadPos(delta, pos2);
//				dists[n1][n2] = dists[n2][n1] = sqrt(delta.sumOfSqures());
//				//			std::cout << dists[n1][n2] << std::endl;
//			}
//			dists[n1][n1] = 0;
//		}
//		for (int n1 = 0; n1 < NumP_; n1++) {
//			ippsSortIndexAscend_64f_I(dists[n1], indexTemp, NumP_);
//			for (int i = 0; i < numOfnn; i++) {
//				nnIndices[m][n1][i] = indexTemp[i + 1];
//				p = nnIndices[m][n1][i];
//			}
//		}
//	}
//}
////////////////////////
// World Line Force Data
///////////////////////
WorldLinesForce::WorldLinesForce(int NumP, int M)  {
	forceData_.resize(NumP,M);
	jumpForceData_.resize(NumP,M);
	totalForce_.resize(M);
	std::fill(totalForce_.begin(), totalForce_.end(), 0);
	totalForceJump_ = 0;
}
void WorldLinesForce::resize(int NumP, int M) {
	forceData_.resize(NumP,M);
//	jumpForceDataAlloc_ = new double[NumP * dim];
//	totalForceAlloc_ = new double[M];
//	jumpForceData_ = jumpForceDataAlloc_;
//	totalForce_ = totalForceAlloc_;
	jumpForceData_.resize(NumP);
	totalForce_.resize(M, 0);
	totalForceJump_ = 0;
}
WorldLinesForce::~WorldLinesForce() {
}
double WorldLinesForce::getTotalForceSlice(int m) const {
	return totalForce_[m];
}
double WorldLinesForce::getTotalForceJump() const {
	return totalForceJump_;
}
void WorldLinesForce::setTotalForceSlice(int m, double totalForce) {
	totalForce_[m] = totalForce;
}
void WorldLinesForce::setTotalForceJump(double totalForce) {
	totalForceJump_ = totalForce;
}
void WorldLinesForce::getJumpSliceP(DoublePointerVec & dest) {
	for (int d = 0; d < Dim_; d++)
		dest[d] = jumpForceData_.rowP(d);
}
void WorldLinesForce::swapData(WorldLinesForce &WL2) {
//    int size=this->NumP_*this->M_;
//    cblas_dswap(size, this->jumpForceData_, 1, WL2.jumpForceData_,1);
//    size=this->M_;
//    cblas_dswap(thfis->size(), this->totalForce_, 1, WL2.totalForce_,1);
	std::swap(totalForce_, WL2.totalForce_);
	jumpForceData_.swapData(WL2.jumpForceData_);
	std::swap(totalForceJump_, WL2.totalForceJump_);
	static_cast<WorldLinesData&>(*this).swapData(
			static_cast<WorldLinesData&>(WL2));
}
void WorldLinesForce::updateTotalForce(MoveParams const & moveParams,
		WorldLinesForce const &updateForce) {
	BeadList const &beads = moveParams.beads;
	BeadList::const_iterator it_b;
	for (it_b = (++beads.begin()); it_b != (--beads.end()); it_b++)
		if (it_b->isOddBead())
			totalForce_[it_b->WLSlice_] =
					updateForce.totalForce_[it_b->RWSlice_];
}
void WorldLinesForce::updateForceJump(WorldLinesForce const &updateForce) {
	jumpForceData_ = (updateForce.jumpForceData_);
//ippsCopy_64f(updateForce.jumpForceData_, jumpForceData_, size);
}
void WorldLinesForce::updateMove(MoveParams const & moveParams,
		WorldLinesForce const & forceUpdate, OC isOpenClosed, bool isJumpOdd) {
	this->updateWLSlices(moveParams, forceUpdate);
	this->updateTotalForce(moveParams, forceUpdate);
	if (isOpenClosed == OPEN && isJumpOdd) {
		// Update Forces
		this->updateForceJump(forceUpdate);
		// Update Total Force
		totalForceJump_ = forceUpdate.totalForceJump_;
	} else {
		jumpForceData_.setVal(0);
		totalForceJump_ = 0;
	}
}
void WorldLinesForce::updateSwapMove(MoveParams const & moveParams,
		WorldLinesForce const & forceUpdate, bool isJumpOdd) {
	this->updateWLSlices(moveParams, forceUpdate);
	this->updateTotalForce(moveParams, forceUpdate);
	this->swapWLSegment(moveParams.wlStart, moveParams.wlSwapedStart,
			moveParams.mStart + 1, M_ - 1);
// Update Forces
	if (isJumpOdd % 2) {
		this->updateForceJump(forceUpdate);
		jumpForceData_.swapCol(moveParams.wlStart, moveParams.wlSwapedStart);
		// Update Total Force
		totalForceJump_ = forceUpdate.totalForceJump_;
	} else {
		jumpForceData_.setVal(0);
		totalForceJump_ = 0;
	}
}
double WorldLinesForce::calcTotalForce(int m) {
	DoublePointerVec slice(Dim_);
	this->getSliceP(m, slice);
	double total = 0;
#pragma ivdep
	for (int d = 0; d < Dim_; d++)
		for (int n = 0; n < NumP_; n++)
			total += pow(slice[d][n], 2);
	return total;
}
double WorldLinesForce::calcTotalForceJump() {
	DoublePointerVec slice(Dim_);
	this->getJumpSliceP(slice);
	double total = 0;
#pragma ivdep
	for (int d = 0; d < Dim_; d++)
		for (int n = 0; n < NumP_; n++)
			total += pow(slice[d][n], 2);
	return total;
}
void WorldLinesForce::setJumpForce(int n, DoublePoint const & force) {
	for (int d = 0; d < Dim_; d++) {
		jumpForceData_(d, n) = force[d];
	}
}
void WorldLinesForce::dumpToFile(hdf5RW &hdf5File) const {
	String groupName = "/Force", dsNameTemp;
	hdf5File.CreateGroup(groupName);
	WorldLinesData::dumpToFile(hdf5File, groupName);
	DimsHdf5 dims = boost::assign::list_of(jumpForceData_.col())(
			jumpForceData_.row());
// Jump Force Data
	dsNameTemp = groupName + "/JumpForceData";
	hdf5File.writeComp<double>(dsNameTemp, dims, jumpForceData_.data());
// Total Force
	dims.clear();
	dims.push_back(totalForce_.size());
	dsNameTemp = groupName + "/TotalForce";
	hdf5File.writeComp<double>(dsNameTemp, dims, &(totalForce_[0]));
// Total Jump
// WORM_WL
	dsNameTemp = groupName + "/TotalForceJump";
	hdf5File.writeSingle<double>(dsNameTemp, totalForceJump_);
}
void WorldLinesForce::readFromFile(String fileName) {
	String ds_name;
	hdf5RW hdf5File;
	hdf5File.initRead(fileName);
	// Force Data
	ds_name = "/Force/Data";
	hdf5File.read<double>(ds_name, this->data());
	// Jump Force Data
	ds_name = "/Force/JumpForceData";
	hdf5File.read<double>(ds_name, jumpForceData_.data());
	// Total Force
	ds_name = "/Force/TotalForce";
	hdf5File.read<double>(ds_name, &(totalForce_[0]));
	// Total Force Jump
	ds_name = "/Force/TotalForceJump";
	hdf5File.read<double>(ds_name, &totalForceJump_);
}
void WorldLinesForce::dumpToFile(String fileName) const {
// Open File
	hdf5RW hdf5File;
	hdf5File.initNew(fileName);
	this->dumpToFile(hdf5File);
}
/////////////////////
// World Line Worm
/////////////////////
WorldLinesWorm::WorldLinesWorm(int NumP, int M, int dim) :
		WorldLinesData(NumP, M, dim) {
	tau_ = 0;
	worm_ = 0;
	isOpenClosed_ = CLOSED;
	for (int i = 0; i < NumP; i++)
		Loops_.push_back(i);
}
WorldLinesWorm::~WorldLinesWorm() {
}
void WorldLinesWorm::setWorm(int w, int t, BeadPos const & jumpPos) {
	worm_ = w;
	tau_ = t;
	jumpPos_ = jumpPos;
}
void WorldLinesWorm::setWorm(int w, int t) {
	worm_ = w;
	tau_ = t;
}
int WorldLinesWorm::tau() const {
	return tau_;
}
int WorldLinesWorm::worm() const {
	return worm_;
}
BeadPos WorldLinesWorm::jumpPos() const {
	return jumpPos_;
}

void WorldLinesWorm::setOpenClose(OC isClosed) {
	isOpenClosed_ = isClosed;
}
OC WorldLinesWorm::getOpenClose() const {
	return isOpenClosed_;
}
int WorldLinesWorm::getLoop(int n) const {
	return Loops_[n];
}
IntVec WorldLinesWorm::Loops() {
	return Loops_;
}
void WorldLinesWorm::swapLoops(int n, int m) {
	std::swap(Loops_[n], Loops_[m]);
}
void WorldLinesWorm::setLoop(int n, int m) {
	Loops_[n] = m;
}
int WorldLinesWorm::findPreviousLoop(int n) const {
	IntVec::const_iterator it = std::find(Loops_.begin(), Loops_.end(), n);
	return (it - Loops_.begin());
}
void WorldLinesWorm::calcVelocity(WorldLinesData &velocity) {
// Get Pointer
	DoublePointerVec wlPos(Dim_), vPos(Dim_), lastSlicePos(Dim_),
			lastVelocityPos(Dim_);
	this->getSliceP(0, wlPos);
	this->getSliceP(M_ - 1, lastSlicePos);
	velocity.getSliceP(0, vPos);
	velocity.getSliceP(M_ - 1, lastVelocityPos);
	int numOfBeads = WL::numOfBeads();
// Cacl Differences
	for (int d = 0; d < Dim_; d++) {
		ippsSub_64f(wlPos[d], wlPos[d] + NumP_, vPos[d], NumP_ * (M_ - 1));
		// Calc Diffs of Loops
		for (int n = 0; n < NumP_; n++) {
			lastVelocityPos[d][n] = this->getData(this->getLoop(n), 0, d)
					- this->getData(n, M_ - 1, d);
		}
	}
	velocity.cyclic();
}
// IO
void WorldLinesWorm::dumpToFile(hdf5RW &hdf5File) const {
	String groupName = "/Worm", dsNameTemp;
	hdf5File.CreateGroup(groupName);
	WorldLinesData::dumpToFile(hdf5File, groupName);
// Loops
	DimsHdf5 loops_dims = boost::assign::list_of(NumP_);
	dsNameTemp = groupName + "/Loops";
	hdf5File.writeComp<int>(dsNameTemp, loops_dims, &(Loops_[0]));
// Delta
	DimsHdf5 delta_dims = boost::assign::list_of(Dim_);
	dsNameTemp = groupName + "/JumpPos";
	hdf5File.writeComp<double>(dsNameTemp, delta_dims, jumpPos_.getDataConst());
// Tau
	dsNameTemp = groupName + "/Tau";
	hdf5File.writeSingle<int>(dsNameTemp, this->tau());
// WORM_WL
	dsNameTemp = groupName + "/Worm";
	hdf5File.writeSingle<int>(dsNameTemp, this->worm());
// Is Closed
	dsNameTemp = groupName + "/IsClosed";
	int openClosed = (isOpenClosed_ == CLOSED) ? 1 : 0;
	hdf5File.writeSingle<int>(dsNameTemp, openClosed);
}
void WorldLinesWorm::dumpToFile(String fileName) const {
// Open File
	hdf5RW hdf5File;
	hdf5File.initNew(fileName.c_str());
	this->dumpToFile(hdf5File);
}
void WorldLinesWorm::readFromFile(String fileName) {
	String ds_name;
	hdf5RW hdf5File;
	hdf5File.initRead(fileName);
	ds_name = "/Worm/Data";
	hdf5File.read<double>(ds_name, this->data());
	ds_name = "/Worm/Loops";
	Loops_.resize(NumP_);
	hdf5File.read<int>(ds_name, &(Loops_[0]));
	ds_name = "/Worm/JumpPos";
	hdf5File.read<double>(ds_name, jumpPos_.getData());
// Tau
	ds_name = "/Worm/Tau";
	hdf5File.read<int>(ds_name, &tau_);
// Worm_WL
	ds_name = "/Worm/Worm";
	hdf5File.read<int>(ds_name, &worm_);
// Is Closed
	ds_name = "/Worm/IsClosed";
	int openClosed;
	hdf5File.read<int>(ds_name, &openClosed);
	isOpenClosed_ = (openClosed == 1) ? CLOSED : OPEN;
}
void WorldLinesWorm::calcDistSwap(int n, int mStart, int mEnd, DoubleVec &rho) {
	BeadPos originalBead;
	int numpMoving = rho.size();
	DoublePointerVec endSlice(Dim_);
	Array2D<double> tempVec(Dim_, numpMoving);
	originalBead = WL::getDataPos(n, mStart);
	std::fill(rho.begin(), rho.end(), 0.);
	this->getSliceP(mEnd, endSlice);
	for (int d = 0; d < Dim_; ++d)
		for (int n = 0; n < numpMoving; n++)
			tempVec(d, n) = endSlice[d][n] - originalBead.getD(d);
	cyclic_vec(tempVec.data(), tempVec.size());
	for (int n = 0; n < numpMoving; n++) {
		for (int d = 0; d < Dim_; d++)
			rho[n] += pow(tempVec(d, n), 2);
#if SIM_DIM==2 and defined ROT
		rho[n] += tempVec(0, n) * tempVec(1, n);
#endif
	}
}
;


double WorldLinesWorm::calcEKDiff(BeadList const & beads) const {
	double originalEK = 0, proposedEK = 0;
	BeadPos pos1, pos2, delta;
	BeadList::const_iterator itb = beads.begin(), itb_next = ++beads.begin();
	while (itb_next != beads.end()) {
		// Original
		if (itb->originalJump_)
			pos1 = this->jumpPos();
		else
			pos1 = this->getDataPos(itb->n_, itb->WLSlice_);
		pos2 = this->getDataPos(itb_next->n_, itb_next->WLSlice_);
		pos1.subBeadPos(delta, pos2);
		originalEK += delta.sumOfSqures();
		if (itb->proposedJump_)
			itb_next->beadPos_.subBeadPos(delta, itb->jumpPos_);
		else
			itb_next->beadPos_.subBeadPos(delta, itb->beadPos_);
		proposedEK += delta.sumOfSqures();
		itb++;
		itb_next++;
	}
	return proposedEK - originalEK;
}

// Testing Methods
bool WorldLines::loopCheck(int step, String out_put) {
	return false;
}
bool WorldLines::isValidConf(String out_path) {
	return false;
}
;

//bool WorldLinesWorm::zero_find(int *pos, move_params * MP) {
//	BeadPos size;
//	for (int n = 0; n < NumP_; n++)
//		for (int m = 0; m < M_; m++) {
//			this->get_data_pos(n, m, size);
//			size.set_d(0, fabs(size.get_d(0)));
//			size.set_d(1, fabs(size.get_d(1)));
//			if ((size.get_d(0) < 0.00000001) && (size.get_d(1) < 0.00000001)) {
//				std::cout << "=======" << "\n" << "Zero Bead:" << "\n" << "n="
//						<< n << " m=" << m << " d=" << 0 << "\n" << "=======";
//				std::cout << "Step " << MP->step;
//				pos[0] = n;
//				pos[1] = m;
//				pos[3] = 0;
//				return true;
//			}
//		}
//	return false;
//}

////////////////////////
// World Line Worm with Energy Data
///////////////////////

double WorldLinesWorm::calcEK() {
	double sum;
	int len = this->size() / Dim_, size = this->size();
	WorldLinesData diffs(NumP_, M_, Dim_);
	DoublePointerVec tempPos(Dim_), wlPos(Dim_), wlShiftPos(Dim_),
			lastTempSlice(Dim_);
	this->getSliceP(0, wlPos);
	this->getSliceP(1, wlShiftPos);
	diffs.getSliceP(0, tempPos);
	diffs.getSliceP(M_ - 1, lastTempSlice);
//	String wlFile = "/home/snirgaz/sim_data/Disorder/160611_25/000/WLEK.h5";
//	String tempFile1 =
//			"/home/snirgaz/sim_data/Disorder/160611_25/000/tempFile1.h5";
//	String tempFile2 =
//			"/home/snirgaz/sim_data/Disorder/160611_25/000/tempFile2.h5";
//	String tempFile3 =
//			"/home/snirgaz/sim_data/Disorder/160611_25/000/tempFile3.h5";
//	this->print_to_file(wlFile);
//	hdf5_rw temp1(tempFile1);
//	hsize_t wl_dims[3] = { this->dim(), this->M(), this->NumP() };
//	hsize_t wl_cdims[3] = { 1, 1, this->NumP() };
//	int rank = 3;
//	String ds_name = "\DS1";
	for (int d = 0; d < Dim_; d++) {
		ippsSub_64f(wlPos[d], wlShiftPos[d], tempPos[d], len - NumP_);
		for (int n = 0; n < NumP_; n++) {
			lastTempSlice[d][n] = this->getData(Loops_[n], 0, d)
					- this->getData(n, M_ - 1, d);
		}
		if (isOpenClosed_ == OPEN) {
			double jump = diffs.getData(worm_, tau_, d);
			jump += this->getData(worm_, tau_, d) - jumpPos_.getD(d);
			diffs.setData(worm_, tau_, d, jump);
		}
	}
//	String name1 = "/home/snirgaz/sim_data/Free/310811_25/000/diff1";
//	String name2 = "/home/snirgaz/sim_data/Free/310811_25/000/diff2";
//	String name3 = "/home/snirgaz/sim_data/Free/310811_25/000/diff2";
// Cyclic
//	diffs.dumpToFile(name1);
#pragma ivdep
	cyclic_vec(diffs.data(), size);
//	diffs.dumpToFile(name2);
#ifdef ROT
	double rot_params_[5] = {-1, 1, 0, COS_T, SIN_T};
	cblas_drotm(len, tempPos[0], 1, tempPos[1], 1, rot_params_);
#endif
//	diffs.dumpToFile(name3);
//	temp1.write_double_comp(ds_name, rank, wl_dims, wl_cdims, Temp_);
	std::transform(diffs.data(), diffs.data() + size, diffs.data(),
			[](double d) -> double {return d * d;});
	// ippsSqr_64f_I(diffs.data(), size);
	ippsSum_64f(diffs.data(), size, &sum);
//ippsPowerSpectr_64f(Temp_, Temp_ + len, Temp_, len);
//ippsSum_64f(Temp_, size, &sum);
// Clear Memory
	assert(isInRange(diffs.data(), diffs.size(), 0.2));
	return sum;
}
WorldLines::WorldLines() {
}
WorldLines::WorldLines(int n, int m, int d) :
		WorldLinesWorm(n, m, d), WorldLinesForce(n, m, d) {
}
void WorldLines::dumpToFile(String fileName) const {
	String ds_name;
	hid_t grp, file;
	hdf5RW hdf5File;
	hdf5File.initNew(fileName);
	WorldLinesWorm::dumpToFile(hdf5File);
	WorldLinesForce::dumpToFile(hdf5File);
// EP ,EK
	hdf5File.CreateGroup("Energy");
	ds_name = "/Energy/EP";
	hdf5File.writeSingle<double>(ds_name, WLEnergyData::getEnergy());
	ds_name = "/Energy/EP_V";
	hdf5File.writeSingle<double>(ds_name, WLEnergyData::getEnergyV());
	ds_name = "/Energy/EP_F";
	hdf5File.writeSingle<double>(ds_name, WLEnergyData::getEnergyF());
	ds_name = "/Energy/EK";
	hdf5File.writeSingle<double>(ds_name, WLEnergyData::getEnergyK());
}
void WorldLines::readFromFile(String fileName) {
	String ds_name;
	WorldLinesWorm::readFromFile(fileName);
	WorldLinesForce::readFromFile(fileName);
// EP ,EK
	hdf5RW hdf5File;
	hdf5File.initRead(fileName);
	ds_name = "/Energy/EP";
	hdf5File.read<double>(ds_name, &(WLEnergyData::energy_));
	ds_name = "/Energy/EP_V";
	hdf5File.read<double>(ds_name, &(WLEnergyData::energyV_));
	ds_name = "/Energy/EP_F";
	hdf5File.read<double>(ds_name, &(WLEnergyData::energyF_));
	ds_name = "/Energy/EK";
	hdf5File.read<double>(ds_name, &(WLEnergyData::energyK_));
}

void WorldLines::initializeSolidBCC(String outPath) {
	Bead beadStart, beadEnd;
	beadStart.RWSlice_ = 0;
	beadEnd.RWSlice_ = WorldLinesWorm::M_;
	int n;
	double dx = 1
			/ (pow((double) WorldLinesWorm::NumP_ / 2, 1 / double(WL::Dim_)));
	int np = round(
			(pow((double) WorldLinesWorm::NumP_ / 2, 1 / double(WL::Dim_))));
	double z_shift, shift = 0;
	IntVec pos_int(WL::Dim_);
	BeadPos pos;
	n = 0;
	for (pos_int[2] = 0; pos_int[2] < np * 2; pos_int[2]++) {
		// shift = double(std::rand()) / double(RAND_MAX) * eps;
		pos.setD(2, -0.5 + dx * 0.25 + dx * 0.5 * (double(pos_int[2])) + shift);
		z_shift = (pos_int[2] % 2) ? 0.75 : 0.25;
		for (pos_int[0] = 0; pos_int[0] < np; pos_int[0]++) {
			// shift = double(std::rand()) / double(RAND_MAX) * eps;
			pos.setD(0, -0.5 + dx * (double(pos_int[0]) + z_shift) + shift);
			for (pos_int[1] = 0; pos_int[1] < np; pos_int[1]++) {
				// shift = double(std::rand()) / double(RAND_MAX) * eps;
				pos.setD(1, -0.5 + dx * (double(pos_int[1]) + z_shift) + shift);
				pos.cyclic();
				assert(n < WorldLinesWorm::NumP_);
				for (int m = 0; m < WorldLinesWorm::M_; m++)
					WorldLinesWorm::setDataPos(n, m, pos);
				n++;
			}
		}
	}
	WL::worm_ = 0;
	WL::tau_ = 0;
	WL::jumpPos_ = WL::getDataPos(WL::worm(), WL::tau());
	String outFile = outPath;
	outFile.append("/WLStart");
	this->dumpToFile(outFile);
}
void WorldLines::initializeSolidHCP(String outPath, String latFileInit) {
	boost::multi_array<double, 2> l(boost::extents[WorldLinesWorm::NumP_][3]);
	hdf5RW hdf5File;
	hdf5File.initRead(latFileInit);
	hdf5File.read<double>("/PList", l.origin());
	BeadPos pos;
	for (int n = 0; n < WL::NumP_; n++) {
		for (int d = 0; d < WL::Dim_; d++) {
			pos[d] = l[n][d];
		}
		for (int m = 0; m < WorldLinesWorm::M_; m++)
			WorldLinesWorm::setDataPos(n, m, pos);
	}
	WL::worm_ = 0;
	WL::tau_ = 0;
	WL::jumpPos_ = WL::getDataPos(WL::worm(), WL::tau());
	String outFile = outPath;
	outFile.append("/WLStart");
	this->dumpToFile(outFile);
}
void WorldLines::initializeSolidBCCVacancies(int numOfVac, String outPath,
		QmcRnd &qmcRnd) {
	Bead beadStart, beadEnd;
	beadStart.RWSlice_ = 0;
	beadEnd.RWSlice_ = WorldLinesWorm::M_;
	int n, nCount;
	double dx = 1
			/ (pow(double((WorldLinesWorm::NumP_ + numOfVac) / 2),
					1 / double(WL::Dim_)));
	int np = round(
			(pow(double((WorldLinesWorm::NumP_ + numOfVac) / 2),
					1 / double(WL::Dim_))));
	double z_shift, shift = 0;
	IntVec VacPos;
	IntVec::iterator VacPosIt;
	for (int v = 0; v < numOfVac; v++) {
		bool flag = true;
		while (flag) {
			int pos;
			pos = qmcRnd.normal_.getSingle();
			if (std::find(VacPos.begin(), VacPos.end(), pos) == VacPos.end()) {
				VacPos.push_back(pos);
				flag = false;
			}

		}
	}
	std::sort(VacPos.begin(), VacPos.end());
	IntVec pos_int(WL::Dim_);
	BeadPos pos;
	n = nCount = 0;
	VacPosIt = VacPos.begin();
	for (pos_int[2] = 0; pos_int[2] < np * 2; pos_int[2]++) {
		// shift = double(std::rand()) / double(RAND_MAX) * eps;
		pos.setD(2, -0.5 + dx * 0.25 + dx * 0.5 * (double(pos_int[2])) + shift);
		z_shift = (pos_int[2] % 2) ? 0.75 : 0.25;
		for (pos_int[0] = 0; pos_int[0] < np; pos_int[0]++) {
			// shift = double(std::rand()) / double(RAND_MAX) * eps;
			pos.setD(0, -0.5 + dx * (double(pos_int[0]) + z_shift) + shift);
			for (pos_int[1] = 0; pos_int[1] < np; pos_int[1]++) {
				// shift = double(std::rand()) / double(RAND_MAX) * eps;
				if (n == *VacPosIt && VacPosIt != VacPos.end()
						&& ~VacPos.empty()) {
					VacPosIt++;
					nCount++;
					continue;
				}
				pos.setD(1, -0.5 + dx * (double(pos_int[1]) + z_shift) + shift);
				pos.cyclic();
				assert(n < WorldLinesWorm::NumP_);
				for (int m = 0; m < WorldLinesWorm::M_; m++)
					WorldLinesWorm::setDataPos(n, m, pos);
				n++;
				nCount++;
			}
		}
	}
	WL::worm_ = 0;
	WL::tau_ = 0;
	WL::jumpPos_ = WL::getDataPos(WL::worm(), WL::tau());
	String outFile = outPath;
	outFile.append("/WLStart");
	this->dumpToFile(outFile);
}
void WorldLines::initializeSolidBCCInterstitials(int numOfInter, String outPath,
		QmcRnd &qmcRnd) {
	Bead beadStart, beadEnd;
	beadStart.RWSlice_ = 0;
	beadEnd.RWSlice_ = WorldLinesWorm::M_;
	int n, nCount;
	double dx = 1
			/ (pow(double((WorldLinesWorm::NumP_ - numOfInter) / 2),
					1 / double(WL::Dim_)));
	int np = round(
			(pow(double((WorldLinesWorm::NumP_ - numOfInter) / 2),
					1 / double(WL::Dim_))));
	double z_shift, shift = 0;
	IntVec InterPos;
	IntVec::iterator InterPosIt;
	for (int v = 0; v < numOfInter; v++) {
		bool flag = true;
		while (flag) {
			int pos;
			pos = qmcRnd.normal_.getSingle();
			if (std::find(InterPos.begin(), InterPos.end(), pos)
					== InterPos.end()) {
				InterPos.push_back(pos);
				flag = false;
			}

		}
	}
	std::sort(InterPos.begin(), InterPos.end());
	IntVec pos_int(WL::Dim_);
	BeadPos pos;
	n = nCount = 0;
	InterPosIt = InterPos.begin();
	for (pos_int[2] = 0; pos_int[2] < np * 2; pos_int[2]++) {
		// shift = double(std::rand()) / double(RAND_MAX) * eps;
		pos.setD(2, -0.5 + dx * 0.25 + dx * 0.5 * (double(pos_int[2])) + shift);
		z_shift = (pos_int[2] % 2) ? 0.75 : 0.25;
		for (pos_int[0] = 0; pos_int[0] < np; pos_int[0]++) {
			// shift = double(std::rand()) / double(RAND_MAX) * eps;
			pos.setD(0, -0.5 + dx * (double(pos_int[0]) + z_shift) + shift);
			for (pos_int[1] = 0; pos_int[1] < np; pos_int[1]++) {
				// shift = double(std::rand()) / double(RAND_MAX) * eps;
				if (n == *InterPosIt && InterPosIt != InterPos.end()
						&& ~InterPos.empty()) {
					pos.setD(1,
							-0.5 + 0.5 * dx * (double(pos_int[1]) + z_shift)
									+ shift);
					pos.cyclic();
					assert(n < WorldLinesWorm::NumP_);
					for (int m = 0; m < WorldLinesWorm::M_; m++)
						WorldLinesWorm::setDataPos(n, m, pos);
					n++;
					nCount++;
					InterPosIt++;
					if (n != nCount)
						std::cout << "Hello";

				}
				pos.setD(1, -0.5 + dx * (double(pos_int[1]) + z_shift) + shift);
				pos.cyclic();
				assert(n < WorldLinesWorm::NumP_);
				for (int m = 0; m < WorldLinesWorm::M_; m++)
					WorldLinesWorm::setDataPos(n, m, pos);
				n++;
				nCount++;
				if (n != nCount)
					std::cout << "Hello";
			}
		}
	}
	WL::worm_ = 0;
	WL::tau_ = 0;
	WL::jumpPos_ = WL::getDataPos(WL::worm(), WL::tau());
	WL::isOpenClosed_ = CLOSED;
	String outFile = outPath;
	outFile.append("/WLStart");
	this->dumpToFile(outFile);
}
void WorldLines::initializeRandom(String outPath, QmcRnd &qmcRnd) {
	BeadPos pos;
	for (int n = 0; n < WorldLinesWorm::NumP_; n++) {
		for (int d = 0; d < WL::Dim_; d++)
			pos.setD(d, qmcRnd.uniZeroOne_.getSingle() - .5);
		for (int m = 0; m < WorldLinesWorm::M_; m++)
			WorldLinesWorm::setDataPos(n, m, pos);
	}
	WL::worm_ = 0;
	WL::tau_ = 0;
	WL::jumpPos_ = WL::getDataPos(WL::worm(), WL::tau());
	String OutFile = outPath;
	WL::isOpenClosed_ = CLOSED;
	OutFile.append("/WLStart");
	this->dumpToFile(OutFile);

}
void WorldLines::initializeSolid(String outPath) {
	double dx = 1 / (pow((double) WorldLinesWorm::NumP_, 1 / double(WL::Dim_)));
	int np = round((pow((double) WorldLinesWorm::NumP_, 1 / double(WL::Dim_)))),
			r, m;
	double shift = 0, eps = 0.000001;
	IntVec pos_int(WL::Dim_);
//double pos[2] = { -0.5 - (0.5 * dx), -0.5 - (0.5 * dx) };
	BeadPos pos;
	for (int d = 0; d < WL::Dim_; d++) {
		// shift = double(std::rand()) / double(RAND_MAX) * eps;
		pos.setD(d, 0);
		pos_int[d] = 0;
	}
	for (int n = 0; n < WorldLinesWorm::NumP_; n++) {
		for (int m = 0; m < WorldLinesWorm::M_; m++)
			WorldLinesWorm::setDataPos(n, m, pos);
		r = 1;
		for (int d = 0; d < WL::Dim_; d++) {
			// shift = double(std::rand()) / double(RAND_MAX) * eps;
			if (r) {
				pos_int[d]++;
				pos.addD(d, dx);
				pos.cyclic();
			}
			r = pos_int[d] / np;
			if (r) {
				pos.setD(d, 0);
				pos_int[d] = 0;
			}
		}
	}
	WL::worm_ = 0;
	WL::tau_ = 0;
	WL::jumpPos_ = WL::getDataPos(WL::worm(), WL::tau());
	String OutFile = outPath;
	OutFile.append("/WLStart");
	this->dumpToFile(OutFile);
}
void WorldLines::initializeSolidFixed(String outPath) {
	double dx = 1 / (pow((double) WorldLinesWorm::NumP_, 1 / double(WL::Dim_)));
	int np = round((pow((double) WorldLinesWorm::NumP_, 1 / double(WL::Dim_)))),
			r, m;
	double shift = 0, eps = 0.000001;
	IntVec pos_int(WL::Dim_);
//double pos[2] = { -0.5 - (0.5 * dx), -0.5 - (0.5 * dx) };
	BeadPos pos;
	for (int d = 0; d < WL::Dim_; d++) {
		// shift = double(std::rand()) / double(RAND_MAX) * eps;
		pos.setD(d, -0.5 + dx / 2 + shift);
		pos_int[d] = 0;
	}
	for (int n = 0; n < WorldLinesWorm::NumP_; n++) {
		for (int m = 0; m < WorldLinesWorm::M_; m++)
			WorldLinesWorm::setDataPos(n, m, pos);
		r = 1;
		for (int d = 0; d < WL::Dim_; d++) {
			// shift = double(std::rand()) / double(RAND_MAX) * eps;
			if (r) {
				pos_int[d]++;
				pos.setD(d, pos.getD(d) + dx + shift);
			}
			r = pos_int[d] / np;
			if (r) {
				pos.setD(d, -0.5 + dx / 2 + shift);
				pos_int[d] = 0;
			}
		}
	}
	WL::worm_ = 0;
	WL::tau_ = 0;
	WL::jumpPos_ = WL::getDataPos(WL::worm(), WL::tau());
	int nLast = WL::NumP_ - 2 * np - 1;
	for (int n = 0; n < 2 * np; n++) {
		WL::swapWLSegment(nLast, n, 0, WL::M_ - 1);
		nLast--;
	}
	String OutFile = outPath;
	OutFile.append("/WLStart");
	this->dumpToFile(OutFile);
}
void WorldLines::swapData(WorldLines &WL2) {
	static_cast<WorldLinesWorm&>(*this).swapData(
			static_cast<WorldLinesWorm&>(WL2));
	static_cast<WorldLinesForce&>(*this).swapData(
			static_cast<WorldLinesForce&>(WL2));
	static_cast<WLEnergyData&>(*this).swapData(static_cast<WLEnergyData&>(WL2));
}

WLEnergyData& WorldLines::getEnergyData() {
	return static_cast<WLEnergyData&>(*this);
}

void WorldLines::updateMove(MoveParams const &moveParams,
		WorldLinesForce const &forceUpdate, EnergyData const &energyDiff,
		OC isOpenClosed) {
	WLEnergyData::updateEnergyK(WL::calcEKDiff(moveParams.beads));
	WorldLinesWorm::updateMove(moveParams, isOpenClosed);
#ifndef POT_FREE
	WorldLinesForce::updateMove(moveParams, forceUpdate, isOpenClosed,
			WL::tau() % 2);
#endif
	WLEnergyData::update(energyDiff);
}
void WorldLines::updateSwapMove(MoveParams const &moveParams,
		WorldLinesForce const &forceUpdate, EnergyData energyDiff) {
	WLEnergyData::updateEnergyK(WL::calcEKDiff(moveParams.beads));
	WorldLinesWorm::updateSwapMove(moveParams);
#ifndef POT_FREE
	WorldLinesForce::updateSwapMove(moveParams, forceUpdate, WL::tau() % 2);
#endif
	WLEnergyData::update(energyDiff);
}

void WorldLines::initializeSolidBCCFixedGB(String outPath, String latFileInit,
		int &numOfFixed) {
	double dx = 1
			/ (pow((double) WorldLinesWorm::NumP_ / 2, 1 / double(WL::Dim_)));
	int np = round(
			(pow((double) WorldLinesWorm::NumP_ / 2, 1 / double(WL::Dim_))));
	boost::multi_array<double, 2> l(boost::extents[WorldLinesWorm::NumP_][3]);
	hdf5RW hdf5File;
	hdf5File.initRead(latFileInit);
	hdf5File.read<double>("/PList", l.origin());
	hdf5File.read<int>("/NumFixed", &numOfFixed);
	BeadPos pos;
	for (int n = 0; n < WL::NumP_; n++) {
		for (int d = 0; d < WL::Dim_; d++) {
			pos[d] = l[n][d];
		}
		for (int m = 0; m < WorldLinesWorm::M_; m++)
			WorldLinesWorm::setDataPos(n, m, pos);
	}
	WL::worm_ = 0;
	WL::tau_ = 0;
	WL::jumpPos_ = WL::getDataPos(WL::worm(), WL::tau());
	String outFile = outPath;
	outFile.append("/WLStart");
	this->dumpToFile(outFile);
}
